#include "search.h"
#include "math.h"
#if (KEIL)
#include "SEEKFREE_MT9V03X.h"
#endif

int IMAGE_MIDDLE = 130;
/********************************************变量声明***************************************************/
uint8 g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT]; // 图像原始数据，g_VideoImageData[m][n]代表图像的从下数第m行，从右数第n点，0为最黑，255为最白
//==============================图像数据================================//
_POINT g_CenterPosition[MAX_POINT_NUM];
_POINT g_LeftEdge[MAX_POINT_NUM], g_RightEdge[MAX_POINT_NUM];
int g_CenterNum;
int g_LeftEdgeNum, g_RightEdgeNum;
//==============================图像处理用变量==========================//
uint16 g_Start[10], g_End[10], g_SEnum;
uint8 g_SearchFlag = 1;
int g_Cover;
uint8 g_CoverIndex[10], g_CoverNum;
//==============================控制用变量==============================//
int g_DirectionControl = MID_VIDEO_POINT;
int g_FormerDirectionControl = MID_VIDEO_POINT;
int g_DirectionControlWhole = 0;
int g_DirectionControlLine = 0;

//==============================新添的变量==============================//

int banma_flag=0;
int Flag = 0;
bool FlagStraightLine = 0;
int StopFlag = 0;
int FlagCarage[5] = { 0 };  // 状态机数组
// 左前拐点是否存在的标志位,1表示消失，0表示存在
bool FlagLeftDown = 0, LeftTemp = 0;
bool FlagRightEdgeOff = 0;  // 低行右边边线消失
bool FlagRightEdgeOn = 0;   // 低行右边边线重新出现
bool FlagLeftEdgeOn = 0;    // 左边边线重新出现
//======================================================================//

/*自适应动态阈值范围，获得一个相对最佳的阈值*/
void IMAGE_MIDDLE_SET()
{
	// 大津法
	unsigned long Amount = 0;
	unsigned long PixelBack = 0;
	unsigned long PixelshortegralBack = 0;
	unsigned long Pixelshortegral = 0;
	signed long PixelshortegralFore = 0;
	signed long PixelFore = 0;
	float OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差
	signed short MinValue = 0, MaxValue = 255;
	unsigned char HistoGram[256] = { 0 };
	// g_VideoImageData[MAX_VIDEO_LINE][MAX_VIDEO_POINT]
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
			HistoGram[g_VideoImageData[i][j]]++;
	for (; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++);
	for (; MaxValue > MinValue && HistoGram[MaxValue] == 0 && MaxValue!=MinValue; MaxValue--);
	if (MaxValue == MinValue)
	{
		IMAGE_MIDDLE = MaxValue;
	}
	else
	{
		if ((MinValue + 1) == MaxValue)
		{
			IMAGE_MIDDLE = MinValue;
		}
		else
		{
			Pixelshortegral = 0;
			SigmaB = -1;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				Amount += HistoGram[i];  // 像素点总数
				Pixelshortegral += HistoGram[i] * i;  // 灰度值总数
			}
			float tmp = Pixelshortegral;
			float average_pixel = tmp / Amount;
			for (int i = MinValue; i <= MaxValue; i++)
			{
				PixelBack  = PixelBack + HistoGram[i];  // 前景像素点数
				PixelFore = Amount - PixelBack;  // 背景像素点数
				OmegaBack = ((float)PixelBack) / Amount;  // 前景像素百分比
				OmegaFore = ((float)PixelFore) / Amount;  // 背景像素百分比
				PixelshortegralBack += HistoGram[i] * i; // 前景灰度值
				PixelshortegralFore = Pixelshortegral - PixelshortegralBack;  // 背景灰度值
				MicroBack = ((float)PixelshortegralBack) / PixelBack;  // 前景灰度百分比
				MicroFore = ((float)PixelshortegralFore) / PixelFore;  // 背景灰度百分比
				Sigma = PixelBack * (MicroBack - average_pixel) * (MicroBack - average_pixel) + PixelFore * (MicroFore - average_pixel) * (MicroFore - average_pixel);  // 类间方差
				if (Sigma > SigmaB)
				{
					SigmaB = Sigma;
					IMAGE_MIDDLE = i;
				}
			}
		}
	}
}


/*********	search:处理数据即测试算法的函数 ********/
void Search()
{
	//=======================图像识别=========================//
	// PS：此时我们的图像已经提取完毕 我们从底部开始，向上识别图像
	// 图像识别数据初始化
	int i, line, white_width = 0;
	g_CenterNum = 0;
	g_LeftEdgeNum = 0;
	g_RightEdgeNum = 0;
	g_SearchFlag = 1;
	IMAGE_MIDDLE_SET();

	for (line = 0; line <= MAX_VIDEO_LINE - 1; line++)
	{
		// 首先 求出本行所有白块的位置
		g_SEnum = 0;													// 本行的白块计数器清零
		if (g_VideoImageData[line][MAX_VIDEO_POINT - 1] > IMAGE_MIDDLE) // 大于阈值为白 小于阈值为黑
			g_Start[g_SEnum] = 0;
		for (i = MAX_VIDEO_POINT - 1; i > 0; i--)
		{
			if (g_VideoImageData[line][i] > IMAGE_MIDDLE && g_VideoImageData[line][i - 1] <= IMAGE_MIDDLE)
			{
				g_End[g_SEnum] = MAX_VIDEO_POINT - i;
				g_SEnum++;
			}
			else if (g_VideoImageData[line][i] <= IMAGE_MIDDLE && g_VideoImageData[line][i - 1] > IMAGE_MIDDLE)
			{
				g_Start[g_SEnum] = MAX_VIDEO_POINT - i - 1;
			}
			else if (g_SEnum >= 10)
			{
				break;
			}
		}
		if (g_VideoImageData[line][0] > IMAGE_MIDDLE && g_SEnum < 10)
		{
			g_End[g_SEnum] = MAX_VIDEO_POINT - 1;
			g_SEnum++;
		}
		// 把最下面的那一行单独拿出来处理 做为后续处理的基础
		if (g_SearchFlag) // 最下一行处理标志
		{
			int temp_mark = 0;
			if (g_SEnum == 0)
			{
				continue;
			}
			white_width = g_End[0] - g_Start[0];
			for (i = 1; i < g_SEnum; i++) // 直接求最宽的白块
			{
				if (g_End[i] - g_Start[i] >= white_width)
				{
					white_width = g_End[i] - g_Start[i];
					temp_mark = i;
				}
			}
			if (white_width > WIDTH)
			{
				g_SearchFlag = 0;
				// 认为第一行最宽的白块便是所需白块
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
		else
		{
			if (g_SEnum == 0)
			{
				break;
			}
			for (i = 0, g_CoverNum = 0; i < g_SEnum; i++)
			{
				// 覆盖关系，因为图像是一个联通区域 于是找与上一行已经找到白块相联通的白块
				g_Cover = MIN(g_End[i], g_RightEdge[g_RightEdgeNum - 1].y) - MAX(g_Start[i], g_LeftEdge[g_LeftEdgeNum - 1].y);
				if (g_Cover > -1)
				{
					g_CoverIndex[g_CoverNum] = i;
					g_CoverNum++;
				}
			}
			if (g_CoverNum == 1) // 如果只有一个联通的 直接取出这个联通白块
			{
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[g_CoverIndex[0]];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[g_CoverIndex[0]];
			}
			else if (g_CoverNum == 0) // 没有联通的白块 说明图像已经处理完毕
			{
				break; // 跳出整个图像处理的循环 进入求中心部分
			}
			else // 有多个白块   需要取舍
			{
				// 取舍规则：先求出本行的白块的中心，再寻找与上一行白块中心最相近的一个白块
				int temp_mark, temp_dis, temp_center, last_center;
				temp_mark = g_CoverIndex[0];
				temp_center = (g_Start[g_CoverIndex[0]] + g_End[g_CoverIndex[0]]) / 2;
				last_center = (g_LeftEdge[g_LeftEdgeNum - 1].y + g_RightEdge[g_RightEdgeNum - 1].y) / 2;
				temp_dis = ABS(last_center - temp_center);
				for (i = 1; i < g_CoverNum; i++)
				{
					temp_center = (g_Start[g_CoverIndex[i]] + g_End[g_CoverIndex[i]]) / 2;
					if (ABS(temp_center - last_center) < temp_dis)
					{
						temp_dis = ABS(temp_center - last_center);
						temp_mark = i;
					}
				}
				g_LeftEdge[g_LeftEdgeNum].x = line;
				g_LeftEdge[g_LeftEdgeNum++].y = g_Start[temp_mark];
				g_RightEdge[g_RightEdgeNum].x = line;
				g_RightEdge[g_RightEdgeNum++].y = g_End[temp_mark];
			}
		}
	}

	//=======================起点判断========================//
		int banma_jishu = 0;
	
		for (line = 0; line <= MAX_VIDEO_LINE - 10; line++) {
			g_SEnum = 0;
			if (g_VideoImageData[line][MAX_VIDEO_POINT - 1] < IMAGE_MIDDLE) 
				g_Start[g_SEnum] = 0;
			for (i = MAX_VIDEO_POINT - 1; i > 0; i--)
			{
				if (g_VideoImageData[line][i] <= IMAGE_MIDDLE && g_VideoImageData[line][i - 1] > IMAGE_MIDDLE)
				{
					g_End[g_SEnum] = MAX_VIDEO_POINT - i;
					g_SEnum++;
				}
				else if (g_VideoImageData[line][i] > IMAGE_MIDDLE && g_VideoImageData[line][i - 1] <= IMAGE_MIDDLE)
				{
					g_Start[g_SEnum] = MAX_VIDEO_POINT - i - 1;
				}
			}
			if (g_VideoImageData[line][0] <= IMAGE_MIDDLE)
			{
				g_End[g_SEnum] = MAX_VIDEO_POINT - 1;
				g_SEnum++;
			}
			if(g_SEnum>=5)
			{
				banma_jishu++;
			}				
	}

	if (banma_jishu >= 6)
		banma_flag=1;
	//=======================十字处理========================//
	//=======================中心求取========================//
	/*
	*
	此处的中心求取非常简单粗糙，因为之前在边沿搜索的时候并没有区分两边，
	而是直接把一个白块的start认为是左边，end认为是右边 故中心数将与左右
	边的数相等，而中心位置直接由左右边相加除2所得
	*
	*/
	
	Flag = CharacterSearch();
	
	
	g_CenterNum = g_RightEdgeNum;
	for (i = 0; i < g_RightEdgeNum; i++)
	{
		g_CenterPosition[i].x = g_RightEdge[i].x;
		g_CenterPosition[i].y = (g_RightEdge[i].y + g_LeftEdge[i].y) / 2;
	}
	

	//===================控制中心及电机控制====================//
	// 加权平均法求方向控制
	g_DirectionControlWhole = 0;
	g_DirectionControlLine = 0;
	for (i = 0; i < g_CenterNum; i++)
	{
		if (g_CenterPosition[i].y >= 0 && g_CenterPosition[i].y <= MAX_VIDEO_POINT)
		{
			g_DirectionControlLine += (int)g_CenterPosition[i].x;
			g_DirectionControlWhole += (int)g_CenterPosition[i].y * g_CenterPosition[i].x; // 注意数据不要溢出
		}
	}
	if (g_DirectionControlLine > 0)
	{
		g_DirectionControl = g_DirectionControlWhole / g_DirectionControlLine;
	}
	// 方向控制限定
	if (g_DirectionControl < 0)
	{
		g_DirectionControl = 0;
	}
	else if (g_DirectionControl > MAX_VIDEO_POINT)
	{
		g_DirectionControl = MAX_VIDEO_POINT;
	}
	if (ABS(g_DirectionControl - g_FormerDirectionControl) > 90)
	{
		g_DirectionControl = g_FormerDirectionControl;
	}
}

/*特征提取*/
int CharacterSearch()
{
	// 提取图像特征
	// 类直道或直道,白块从第一行延伸到最后一行
	if (g_LeftEdgeNum == g_RightEdgeNum && g_LeftEdgeNum == MAX_VIDEO_LINE)
	{
		// 最小二乘法
		int LeftEdgeBreakLoop = 0;  // 左边线开始退出循环的行数
		int LoopVariate = 0;  // 循环变量
		int LeftLineCount = 0, RightLineCount = 0;  // 计数
		// 遍历左边线数组
		int left_i = 0, Right_i = 0;
		float sum_x = 0, sum_y = 0, sum_xx = 0, sum_xy = 0, Slope, Intercept;
		for (; LoopVariate < g_LeftEdgeNum; LoopVariate++)
		{
			sum_x += (float)(g_LeftEdge[LoopVariate].x);
			sum_y += (float)(g_LeftEdge[LoopVariate].y);
			sum_xx += (float)(g_LeftEdge[LoopVariate].x * g_LeftEdge[LoopVariate].x);
			sum_xy += (float)(g_LeftEdge[LoopVariate].x * g_LeftEdge[LoopVariate].y);
			if (g_LeftEdge[LoopVariate].y >= 0 && g_RightEdge[LoopVariate].y == 161)  // 只有左线像素
			{
				LeftLineCount++;
				left_i++;
			}
			if (g_LeftEdge[LoopVariate].y == 0 && g_RightEdge[LoopVariate].y <= 161)  // 只有右线像素
			{
				RightLineCount++;
				Right_i++;
			}
		}
		Slope = (sum_xy - sum_x * sum_y / g_LeftEdgeNum) / (sum_xx - sum_x * sum_x / g_LeftEdgeNum);  // 斜率
		Intercept = sum_y / g_LeftEdgeNum - Slope * (sum_x / g_LeftEdgeNum);  // 截距
		float error = 0;
		for (int i = 0; i < g_LeftEdgeNum; i++)
		{
			error += (Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y) * (Slope * g_LeftEdge[i].x + Intercept - g_LeftEdge[i].y);
		}
		if (error < 138 && error > 0)
		{
			FlagStraightLine = 1;  // 直道
			return FlagStraightLine;
		}
		else
			return 0;
	}
	else
	{
		return -1;
	}
}

/*入库算法*/
void GoToCarage()
{
	// 左入库
	int countRightEdgeOff = 0, countRightEdgeOn = 0, countLeftEdgeOff = 0, countLeftEdgeOn = 0;
	int Index = 0;
	for (int i = 1; i < g_LeftEdgeNum-1; i++)
	{
		// 判断左前拐点是否消失
		if (g_LeftEdge[i].y > 0 && g_LeftEdge[i + 1].y == 0 && ABS(g_LeftEdge[i - 1].y - g_LeftEdge[i].y) < 2 && LeftTemp == 0)
		{
			LeftTemp = 1;
			Index = i;
		}
		if (g_LeftEdge[i].y == 0 && i < Index)
		{
			countLeftEdgeOff += 1;
		}
		// 判断低行右边边线是否消失
		if (g_RightEdge[i].y == 161 || ABS(g_RightEdge[i].y - 161) < 10)
		{
			countRightEdgeOff += 1;
		}
		// 判断低行右边边线是否重新出现
		if (g_RightEdge[i].y < 161)
		{
			countRightEdgeOn += 1;
		}
		// 判断左边边线重新出现
		if (g_LeftEdge[i].y > 0)
		{
			countLeftEdgeOn += 1;
		}
	}

	if (countRightEdgeOff >= 10)
	{
		FlagRightEdgeOff = 1;
	}
	if (countRightEdgeOn >= 10)
	{
		FlagRightEdgeOn = 1;
	}
	if (countLeftEdgeOff >= 10 && LeftTemp)
	{
		FlagLeftDown = 1;
	}
	if (countLeftEdgeOn >= 10)
	{
		FlagLeftEdgeOn = 1;
	}

	// 状态机数组赋值
	FlagCarage[0] = banma_flag;
	FlagCarage[1] = FlagLeftDown;
	FlagCarage[2] = FlagRightEdgeOff;
	FlagCarage[3] = FlagRightEdgeOn;
	FlagCarage[4] = FlagLeftEdgeOn;

	int tmp = 0;
	for (int i = 0; i < 5; i++)
	{
		if (FlagCarage[i] == 0)
		{
			tmp = 1; // 说明不需要停车
			break;
		}
	}
	if (tmp)
	{
		StopFlag = 0;
	}
	else
	{
		StopFlag = 1;
	}

	// 右入库

}